#include "GamePhysics.h"
#include "GameApp.h"
#include "BtOgreExtras.h"
#include "BtOgreGP.h"
#include "BtOgrePG.h"
#include "OGRE\OgreEntity.h"
#include "Ogre.h"
#include "Convert.h"
#include "CollisionEvent.h"
#include "GameGUI.h"
#include "blobb.h"
#include <cmath>


static const float ORIENTATION_SPEED = 0.15f;
static const float FIN_SPEED = 0.55f;
static const float PI = 3.14159265f;
static const float PI2 = 6.28318531f;

static const float TOP_CLAMP = 1.f - 1e-6f;
static const float BOTTOM_CLAMP = -1.f + 1e-6f;

//-------------------------------------------------------------------------------------
void collisionTickCallback(btDynamicsWorld *world, btScalar timeStep)
{
	// Get the physics of the game
	GamePhysics *physics = static_cast<GamePhysics*>(world->getWorldUserInfo());
	// Collisions from the current tick
	CollisionSet set;
	// Need dispatcher to get all collisions
	btDispatcher *dispatcher = world->getDispatcher();

	// Go through all collisions
	for (int i = 0; i < dispatcher->getNumManifolds(); ++i)
	{
		// Get the manifold, that represents contacts between two objects
		btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);

		// Get objects that have a contact between each other
		btRigidBody *body0 = static_cast<btRigidBody*>(manifold->getBody0());
		btRigidBody *body1 = static_cast<btRigidBody*>(manifold->getBody1());
		GameMotionState *state0 = static_cast<GameMotionState*>(body0->getMotionState());
		GameMotionState *state1 = static_cast<GameMotionState*>(body1->getMotionState());
		const ObjectPtr obj0 = state0->getObject();
		const ObjectPtr obj1 = state1->getObject();

		int tmp = manifold->getNumContacts();
		for (int i = 0; i < tmp; ++i)
		{
			if (manifold->getContactPoint(i).getDistance() < 0.005f)
			{
				// Add the collison in the current collisionset
				CollisionPair pair(body0, body1);
				set.insert(pair);
		
				// Check if this is a new collision
				//if (!physics->getLastCollisionSet().count(pair))
				{
					// Send event
					physics->sendCollisionEvent(obj0, obj1, manifold);
				}
				
				break;
			}
		}
	}

	// Set current collisionset to last collisionset
	physics->setLastCollisionSet(set);
}
//-------------------------------------------------------------------------------------
GamePhysics::GamePhysics(void)
	: mpBroadphase(0), mpCollisionConfiguration(0), mpDispatcher(0), mpSolver(0), mpDynamicsWorld(0), mpDebugDrawer(0), mpSceneMgr(0)
{
	mFinIntersection = btVector3();
}
//-------------------------------------------------------------------------------------
GamePhysics::~GamePhysics(void)
{
	delete mpStaticBody;
	delete mpDebugDrawer;
	delete mpDynamicsWorld;
    delete mpSolver;
    delete mpCollisionConfiguration;
    delete mpDispatcher;
    delete mpBroadphase;
}
//-------------------------------------------------------------------------------------
bool GamePhysics::init(Ogre::SceneManager* scm)
{
	mpSceneMgr = scm;
	mpBroadphase = new btDbvtBroadphase();
    mpCollisionConfiguration = new btDefaultCollisionConfiguration();
    mpDispatcher = new btCollisionDispatcher(mpCollisionConfiguration);
    mpSolver = new btSequentialImpulseConstraintSolver;
    mpDynamicsWorld = new btDiscreteDynamicsWorld(mpDispatcher,mpBroadphase,mpSolver,mpCollisionConfiguration);
	mpDynamicsWorld->setGravity(btVector3(0.0f,0.0f,0.0f));
	mpDebugDrawer = new BtOgre::DebugDrawer(scm->getRootSceneNode(),mpDynamicsWorld);
	mpDynamicsWorld->setDebugDrawer(mpDebugDrawer);
	mpDynamicsWorld->setInternalTickCallback(collisionTickCallback);
	mpDynamicsWorld->setWorldUserInfo(this);

	mpStaticBody = new btRigidBody(0,NULL,NULL);
	mpStaticBody->setWorldTransform(btTransform::getIdentity());

	return true;
}
//-------------------------------------------------------------------------------------
void GamePhysics::update(const unsigned long elapsedTime)
{
	mpDynamicsWorld->stepSimulation(btScalar(elapsedTime/1000.0), 5, 1.0f / 60.0f / 2.0f);


	// Update intersection between mouse direction and fin
	OIS::MouseState ms = gpApp->mpInput->getMouseState();
	Ogre::Camera *cam = gpApp->mpView->getCamera()->getCamera();
	mCamRight = cvt(cam->getRight());

	Ogre::Ray ogreRay = cam->getCameraToViewportRay(ms.X.abs / (float)ms.width, ms.Y.abs / (float)ms.height);
	mMousePos = cvt(ogreRay.getOrigin());
	btVector3 mouseDir = cvt(ogreRay.getDirection());
	
	btVector3 start = mMousePos + 4.0f * mouseDir;
	btVector3 end = mMousePos + 25.0f * mouseDir;
	btVector3 norm;
}
//-------------------------------------------------------------------------------------
void GamePhysics::getDirectionVecs(btVector3 &pos, btVector3 &right) const
{
	pos = mMousePos;
	right = mCamRight;
}
//-------------------------------------------------------------------------------------
void GamePhysics::addBody(BaseObjectPtr &obj)
{
	// Get all needed data for Bullet
	BulletDataPtr data = obj->getBulletInitData();
	// If needed, create an extra node for the object to be able to set transformation in Ogre
	if (data->extraNode)
	{
		Ogre::SceneNode *entityNode = obj->getEntityNode();
		Ogre::SceneNode *node = entityNode->createChildSceneNode();
		// Move entity of the old node to the new one
		Ogre::Entity *entity = obj->getEntity();
		entityNode->detachObject(entity);
		node->attachObject(entity);
		// Reset nodes of the object
		obj->setNode(entityNode);
		obj->setEntityNode(node);
	}
	else obj->setNode(obj->getEntityNode());

	Ogre::Entity* ent;
	if(*(obj->getBulletInitData()->collisionMeshName) == Ogre::String(""))
	{
		ent = obj->getEntity();
	}
	else
	{
		Ogre::Entity* entity;
		if (mpSceneMgr->hasEntity(*(data->collisionName)))
		{
			entity = mpSceneMgr->getEntity(*(data->collisionName));
		}
		else
		{
			entity = mpSceneMgr->createEntity(*(data->collisionName), *(data->collisionMeshName));
		}
		obj->getNode()->attachObject(entity);
		entity->setVisible(false);
		ent = entity;
	}
	BtOgre::StaticMeshToShapeConverter converter(ent);
	if (obj->getBulletInitData()->shape == SHAPE_SPHERE)
	{
		btCollisionShape* objShape = converter.createSphere();
		GameMotionState* state = new GameMotionState(ObjectPtr(obj), cvt(converter.getOffset()));
		btVector3 inertia(0,0,0);
		objShape->calculateLocalInertia(obj->getBulletInitData()->mass, inertia);
		btRigidBody::btRigidBodyConstructionInfo info(obj->getBulletInitData()->mass, state, objShape, inertia);
		btRigidBody* body = new btRigidBody(info);
		mpDynamicsWorld->addRigidBody(body, obj->getBulletInitData()->type, obj->getBulletInitData()->mask);
		
		obj->setBody(body);
	}
	if (obj->getBulletInitData()->shape == SHAPE_CAPSULE)
	{
		btCollisionShape* objShape = converter.createCapsule();
		GameMotionState* state = new GameMotionState(ObjectPtr(obj), cvt(converter.getOffset()));
		btVector3 inertia(0,0,0);
		objShape->calculateLocalInertia(obj->getBulletInitData()->mass, inertia);
		btRigidBody::btRigidBodyConstructionInfo info(obj->getBulletInitData()->mass, state, objShape, inertia);
		btRigidBody* body = new btRigidBody(info);
		mpDynamicsWorld->addRigidBody(body, obj->getBulletInitData()->type, obj->getBulletInitData()->mask);

		obj->setBody(body);
	}
	if (obj->getBulletInitData()->shape == SHAPE_CYLINDER)
	{
		btCollisionShape* objShape = converter.createCylinder();
		GameMotionState* state = new GameMotionState(ObjectPtr(obj), cvt(converter.getOffset()));
		btVector3 inertia(0,0,0);
		objShape->calculateLocalInertia(obj->getBulletInitData()->mass, inertia);
		btRigidBody::btRigidBodyConstructionInfo info(obj->getBulletInitData()->mass, state, objShape, inertia);
		btRigidBody* body = new btRigidBody(info);
		mpDynamicsWorld->addRigidBody(body, obj->getBulletInitData()->type, obj->getBulletInitData()->mask);

		obj->setBody(body);
	}
	else if (obj->getBulletInitData()->shape == SHAPE_BOX)
	{
		btCollisionShape* objShape = converter.createBox();
		GameMotionState* state = new GameMotionState(ObjectPtr(obj), cvt(converter.getOffset()));
		btVector3 inertia(0,0,0);
		objShape->calculateLocalInertia(obj->getBulletInitData()->mass, inertia);
		btRigidBody::btRigidBodyConstructionInfo info(obj->getBulletInitData()->mass, state, objShape, inertia);
		btRigidBody* body = new btRigidBody(info);
		mpDynamicsWorld->addRigidBody(body, obj->getBulletInitData()->type, obj->getBulletInitData()->mask);

		obj->setBody(body);
	}
	else if (obj->getBulletInitData()->shape == SHAPE_MESH)
	{
		btCollisionShape* objShape = converter.createTrimesh();
		GameMotionState* state = new GameMotionState(ObjectPtr(obj));
		btRigidBody::btRigidBodyConstructionInfo info(obj->getBulletInitData()->mass, state, objShape);
		btRigidBody* body = new btRigidBody(info);
		mpDynamicsWorld->addRigidBody(body, obj->getBulletInitData()->type, obj->getBulletInitData()->mask);

		obj->setBody(body);
	}
	else if (obj->getBulletInitData()->shape == SHAPE_CONVEX)
	{
		btCollisionShape* objShape = converter.createConvex();
		GameMotionState* state = new GameMotionState(ObjectPtr(obj), cvt(converter.getOffset()));
		btVector3 inertia(0,0,0);
		objShape->calculateLocalInertia(obj->getBulletInitData()->mass, inertia);
		btRigidBody::btRigidBodyConstructionInfo info(obj->getBulletInitData()->mass, state, objShape, inertia);
		btRigidBody* body = new btRigidBody(info);
		mpDynamicsWorld->addRigidBody(body, obj->getBulletInitData()->type, obj->getBulletInitData()->mask);

		obj->setBody(body);
	}
	obj->getBody()->setRestitution(obj->getBulletInitData()->restitution);
	obj->getBody()->setFriction(obj->getBulletInitData()->friction);
	obj->getBody()->setDamping(obj->getBulletInitData()->linDamp, obj->getBulletInitData()->angDamp);
	obj->getBody()->setAngularFactor(btVector3(obj->getBulletInitData()->angularFactX,
											   obj->getBulletInitData()->angularFactY,
											   obj->getBulletInitData()->angularFactZ));

	if(obj->getBulletInitData()->isGhost)
		obj->getBody()->setCollisionFlags(btCollisionObject::CF_NO_CONTACT_RESPONSE);
	obj->getBody()->getCollisionShape()->setMargin(0.1);
}
//-------------------------------------------------------------------------------------
void GamePhysics::removeBody(btRigidBody *body)
{
	int num=body->getNumConstraintRefs();
	for(int j=0;j<num;++j)
		mpDynamicsWorld->removeConstraint(body->getConstraintRef(0));
	mpDynamicsWorld->removeRigidBody(body);

	delete body->getMotionState();
	delete body;
}
//-------------------------------------------------------------------------------------
btDynamicsWorld *GamePhysics::getDynamicsWorld(void)
{
	return mpDynamicsWorld;
}
//-------------------------------------------------------------------------------------
BtOgre::DebugDrawer *GamePhysics::getDebugDrawer(void)
{
	return mpDebugDrawer;
}
//-------------------------------------------------------------------------------------
CollisionSet GamePhysics::getLastCollisionSet() const
{
	return mLastCollisionSet;
}
//-------------------------------------------------------------------------------------
void GamePhysics::setLastCollisionSet(const CollisionSet set)
{
	mLastCollisionSet = set;
}
//-------------------------------------------------------------------------------------
void GamePhysics::sendCollisionEvent(const ObjectPtr obj0, const ObjectPtr obj1, const btPersistentManifold *manifold) const
{
	const BaseObjectPtr o0 = boost::static_pointer_cast<BaseObject>(obj0);
	const BaseObjectPtr o1 = boost::static_pointer_cast<BaseObject>(obj1);
	btVector3 a = manifold->getContactPoint(0).getPositionWorldOnA();
	btVector3 b = manifold->getContactPoint(0).getPositionWorldOnB();
	btVector3 mid = (a + b) / 2;
	gpApp->mpEventManager->queueEvent(EventPtr(new CollisionEvent(o0, o1, mid)));
}
//-------------------------------------------------------------------------------------
void GamePhysics::applyGravity(BaseObjectPtr obj)
{
	btRigidBody* body;
	bool isCorner;
	int raynumber = 10;
	btTransform &trans = obj->getBody()->getWorldTransform();
	btTransform tmpTrans;
	btVector3 start(obj->getGlobalVector(btVector3(0.f,-0.5f,0.f)));
	btVector3 rotate(0.0f,-10.f,0.0f);
	rotate = trans.getBasis() * rotate;
	btVector3 end(start + rotate);
	btVector3 norm;
	btVector3 axis(0.0,0.0,1.0);
	axis = trans.getBasis() * axis;
	rotate = rotate.rotate(axis, 2.0f * PI * rand()/(float)RAND_MAX);
	btVector3 force(0.0f,0.0f,0.0f);
	bool hit = false;
	for(int i = 0; i < raynumber; i++)
	{
		rotate = rotate.rotate(axis, 2.0f * PI / raynumber);
		end = btVector3(start + rotate);
		if(raycast(start,end,norm, tmpTrans, body,COL_GRAVITY))
		{
			isCorner = ((GameMotionState*)body->getMotionState())->getObject()->getType() == OBJECT_CORNER;
			float cos = norm.dot(tmpTrans.getBasis() * btVector3(0.f,1.f,0.f));
			if(cos > 0.1f || isCorner)
			{
				btVector3 diff(end-start);
				force += -norm/ diff.length() / diff.length();
				hit = true;
			}
		}
	}
	if(hit)
	{
		force.normalize();
		force *= 100.f;
		//force = obj->getBody()->getGravity() * 0.5 + force * 0.5f;
		obj->getBody()->setGravity(force);
	}
}
//-------------------------------------------------------------------------------------
bool GamePhysics::raycast(const btVector3 &Start, btVector3 &End, btVector3 &Normal, short mask) {
			
			btCollisionWorld::ClosestRayResultCallback RayCallback(Start, End);
            RayCallback.m_collisionFilterMask = mask;
            // Perform raycast
            mpDynamicsWorld->rayTest(Start, End, RayCallback);
            if(RayCallback.hasHit()) {

                    End = RayCallback.m_hitPointWorld;
                    Normal = RayCallback.m_hitNormalWorld;
                    return true;
            }

        return false;
}
//-------------------------------------------------------------------------------------
bool GamePhysics::raycast(const btVector3 &Start, btVector3 &End, btVector3 &Normal, btTransform &Trans, short mask) {
			
			btCollisionWorld::ClosestRayResultCallback RayCallback(Start, End);
            RayCallback.m_collisionFilterMask = mask;
            // Perform raycast
            mpDynamicsWorld->rayTest(Start, End, RayCallback);
            if(RayCallback.hasHit()) {

                    End = RayCallback.m_hitPointWorld;
                    Normal = RayCallback.m_hitNormalWorld;
					Trans = btTransform(RayCallback.m_collisionObject->getWorldTransform());
                    return true;
            }

        return false;
}
//-------------------------------------------------------------------------------------
bool GamePhysics::raycast(const btVector3 &Start, btVector3 &End, btVector3 &Normal, btTransform &Trans, btRigidBody*& body, short mask) {
			
			btCollisionWorld::ClosestRayResultCallback RayCallback(Start, End);
            RayCallback.m_collisionFilterMask = mask;
            // Perform raycast
            mpDynamicsWorld->rayTest(Start, End, RayCallback);
            if(RayCallback.hasHit()) {

                    End = RayCallback.m_hitPointWorld;
                    Normal = RayCallback.m_hitNormalWorld;
					Trans = btTransform(RayCallback.m_collisionObject->getWorldTransform());
					body = (btRigidBody*)RayCallback.m_collisionObject;
                    return true;
            }

        return false;
}
//-------------------------------------------------------------------------------------
void GamePhysics::applyOrientation(BaseObjectPtr obj)
{

	btVector3 grav = -(obj->getBody()->getGravity());
	if(grav.length() < 1.f) return;

	grav = grav.normalized();
	Ogre::Matrix3 rotmat;
	btTransform &trans = obj->getBody()->getWorldTransform();
	btVector3 y = trans.getBasis() * btVector3(0.f,1.f,0.f);
	Ogre::Quaternion rot = cvt(y).getRotationTo(cvt(0.7f * y + 0.3f * grav));
	rot.ToRotationMatrix(rotmat);
	btMatrix3x3 btRotmat = cvt(rotmat);
	btQuaternion btRot;
	btRotmat.getRotation(btRot);
	trans.setRotation(btRot * trans.getRotation());
	obj->getBody()->setAngularFactor(0.1f);
}
//-------------------------------------------------------------------------------------
void GamePhysics::snapToRail(BaseObjectPtr obj, bool force)
{
	if(!obj->getJoint())
	{
		btTransform localA,localB;
		localA.setIdentity(); localB.setIdentity();
         
		JointPtr joint6DOF = JointPtr(new btGeneric6DofConstraint(*mpStaticBody, *obj->getBody(), localA, localB, true));

		joint6DOF->setLinearLowerLimit(btVector3(1.f,1.f,0.f));   
		joint6DOF->setLinearUpperLimit(btVector3(-1.f,-1.f,0.f));

		joint6DOF->setAngularLowerLimit(btVector3(0.f,0.f,1.f));
		joint6DOF->setAngularUpperLimit(btVector3(0.f,0.f,-1.f));

		mpDynamicsWorld->addConstraint(joint6DOF.get(),true);
		obj->setJoint(joint6DOF);
	}

	btVector3 normal, start, end, relPos, newOrigin;
	btTransform trans;
	start = obj->getBody()->getWorldTransform().getOrigin();
	end = obj->getGlobalVector(btVector3(0.f,-10.f,0.f));

	btRigidBody* body;
	if(raycast(start, end, normal, trans, body, COL_GRAVITY))
	{
		float angle = normal.dot(trans.getBasis() * btVector3(0.f,1.f,0.f));
		if(angle < 0.f) return;
		Ogre::Matrix3 rotmat;
		btTransform localA,localB;
		localB.setIdentity();
		localA = obj->getJoint()->getFrameOffsetA();
		Ogre::Quaternion rot = Ogre::Vector3(0.f,0.f,1.f).getRotationTo(cvt(trans.getBasis() * btVector3(1.f,0.f,0.f)));
		rot.ToRotationMatrix(rotmat);
		btMatrix3x3 btRotmat = cvt(rotmat);
		btQuaternion btRot;
		btRotmat.getRotation(btRot);
		relPos = obj->getBody()->getWorldTransform().getOrigin() - trans.getOrigin();
		newOrigin = normal * normal.dot(relPos) + trans.getOrigin();
		if(((GameMotionState*)body->getMotionState())->getObject()->getType() == OBJECT_HORIZONTAL_CORNER)
		{
			btRot = btRot * btQuaternion(btVector3(0.f,1.f,0.f), PI * 0.25f);
			//send extra rays to change the position
			end = trans.getOrigin() + trans.getBasis() * btVector3(0.f,0.f,2.f);
			start = end + trans.getBasis() * btVector3(0.f,2.f,0.f);
			if(raycast(start, end, normal, trans, COL_LEVEL))
			{
				newOrigin+=localA.getBasis() * btVector3(0.f,0.f,-0.3f);
			}else
			{
				newOrigin+=localA.getBasis() * btVector3(0.f,0.f,0.3f);
			}
		}
		localA.setRotation(btRot);
		localA.setOrigin(newOrigin);
		obj->getJoint()->setFrames(localA,localB);
	}
}
//-------------------------------------------------------------------------------------
void GamePhysics::forceSnapToRail(BaseObjectPtr obj)
{
}
//-------------------------------------------------------------------------------------
btVector3 GamePhysics::getFinIntersection(void) const
{
	return mFinIntersection;
}